/*
Work in this file belongs to MOTOR CONTROL group. 
Other groups are not allowed to make changes on this file. 

During development, you can use your own test code that
includes your own setup() and loop() functions but they 
should not be included when you are submitting your changes. 

*/

void advance(float powerA, float powerC)  //Motorun temel guc ayarlari ve encoderdan katedilen mesafe olcumu bu fonksiyonda yapilir. powerA ve powerC sag ve sol tekerlege verilecek guc miktarı
                                        //olup, bu miktar cizgi izleme algoritmasi tarafindan belirlenir.
 {   
  if(powerA<0)
    digitalWrite(M1, LOW);
  else
    digitalWrite(M1,HIGH); 
	
  analogWrite (E1, abs(powerA));
   
  if(powerC<0)
    digitalWrite(M2, LOW);
  else
    digitalWrite(M2, HIGH);  
   
  analogWrite (E2, abs(powerC)); 
  
 //encoder icin: 
 static unsigned long timer_1 = 0;               
 if(millis() - timer_1 > 500){
//    Serial.print("Coder value: ");
//    Serial.print(coder[LEFT]);
//    Serial.print("[Left Wheel] ");
//    Serial.print(coder[RIGHT]);
//    Serial.println("[Right Wheel]");
    lastSpeed[LEFT] = coder[LEFT];   //En son hiz kaydedilir.
    lastSpeed[RIGHT] = coder[RIGHT];
    TravelledDistance = TravelledDistance + ((coder[LEFT]+coder[RIGHT])/1.65);  //Katedilen mesafe denklemi.
    Speed = (((coder[LEFT]+coder[RIGHT])/1.65) / 0.5);
//    Serial.print("Travelled Distance= ");
//    Serial.print(TravelledDistance);
//    Serial.println("cm.");
//    Serial.print("Speed= ");
//    Serial.print(Speed);
//    Serial.println("cm/s");
    coder[LEFT] = 0;                 //Data buffer sifirlanir.
    coder[RIGHT] = 0;
    timer_1 = millis();
 }
   
}
//IR detector 
void infrared(int detflag,int goflag){  //Istasyon goruldugunde duzgun durma fonksiyonu. detflag, infrared sensorden gelen bilgiye gore 1 ya da 0 degerini alir. goflag komutu 'Driver'dan gelir.
	if(goflag==0){
		stop();}
	else{
		detflag=1;
		goflag=0;
                //distflag=1;
              }}
              
void stop(void)  //Infrared'de durusu saglar.
{
  digitalWrite(E1,LOW);   
  digitalWrite(E2,LOW);      
}

//Ultrasonic sensor setup fonksiyonu. Bu fonksiyon void setup() fonksiyonu icinde cagrilir ve sensorler yuklenmis olur.
void PWM_Mode_Setup(){ 
  pinMode(URTRIG,OUTPUT);                     // A low pull on pin COMP/TRIG 
  digitalWrite(URTRIG,HIGH);                  
  
  pinMode(URPWM, INPUT);                      // Sending Enable PWM mode command
  
//  for(int i=0;i<4;i++){
//      Serial.write(EnPwmCmd[i]);
//   }
  attachInterrupt(LEFT, LwheelSpeed, CHANGE);    //init the interrupt mode for the digital pin 2
  attachInterrupt(RIGHT, RwheelSpeed, CHANGE); 
}

//Ultrasonic sensorlerin gucunun aktif hale getirilmesi fonksiyonu. Loop fonksiyonu icinde kullanir. 
void PWM_Mode(){                              // a low pull on pin COMP/TRIG  triggering a sensor reading
    digitalWrite(URTRIG, LOW);
    digitalWrite(URTRIG, HIGH);               // reading Pin PWM will output pulses
     
    unsigned long DistanceMeasured=pulseIn(URPWM,LOW);
     
    if(DistanceMeasured!=50000){              // the reading is invalid.
      obstacleDistance=DistanceMeasured/50;           // every 50us low level stands for 1cm
   }
  //Serial.print("Distance=");
  //Serial.print(obstacleDistance);
  //Serial.println("cm");
}

//Sag ve sol encoderlarin hiz olcumunde kullanilmasi icin gerekli fonksiyonlar. Timer suresi icindeki interrupt sayisini hesaplayarak hizi verir.
void LwheelSpeed()
{
  coder[LEFT] ++;  //count the left wheel encoder interrupts
} 
void RwheelSpeed()
{
  coder[RIGHT] ++; //count the right wheel encoder interrupts
}

//void loop() fonksiyonu icinde yukarda tanimlanan fonksiyonlarin cagrilmasi. 
void vehicleStartMoving()  
{
  
  int Infradetect= digitalRead(A3);  //infrared detector pin definition.  
  
  
  if(Infradetect==0){ // Durak okunduysa Infrared fonksıyonuna gıderek calısır. Goonflag=0 sa durur eger durduktan sonra goon 1 yapılırsa , detcet ve goonu 0 ceker .
    infrared(Infradetect,Goonflag);
    TravelledDistance=0;
  }
  if(Infradetect==1 && Goonflag==0){	
     //latest standardline following code should be included her
     stdLineFollowing();
     if(obstacleDistance<15){advance(-1,-1);
                             emergencystop=1;} //emergency stop icin flag
     else
       {advance(slSolMotor,slSagMotor);
        emergencystop=0;}  //emergency stop icin flag
  static unsigned long timer = 0;                //print manager timer
  if(millis() - timer > 500){  //timer'larin zamanlariyla test edilmeden oynanmasi onerilmez. Uzun testler sonucu bu timerlar ayarlanmistir.
    PWM_Mode(); //for ultrasonic meter
    timer = millis();
  }
 }  
}

